package torcs.danielpd.SANE;

import ecprac.ea.abstracts.AbstractDriver;
import ecprac.ea.abstracts.map.Corner;
import ecprac.torcs.client.Action;
import ecprac.torcs.client.SensorModel;
import ecprac.torcs.genome.IGenome;

/**
 * Input units: Distance to next corner, sharpness of next corner, speed, track position (left right middle?) Angle to Track Axis?
 * Output units: Accelerate (yes/no), track position, Align to Axis?
 * @author danielpd
 *
 */
public class MyDriver extends AbstractDriver {
	private String name;
	private DriverGenome genome;
	
	private double acceleration;
	private double steering;
	
	MyDriver (String name) {
		this.name = "Car: " +name;
	}
	
	@Override
	public double getAcceleration(SensorModel sensors) {
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		double[] input = new double[MyEA.inputLayerSize];
		input[0] = sensors.getSpeed();
		input[1] = c.distance;
		input[2] = c.sharpness;
		input[3] = sensors.getTrackPosition();
		input[4] = sensors.getAngleToTrackAxis();
		Neuron[] neurons = genome.hiddenLayer;
		double[] hiddenVals = new double[neurons.length];
		// Calculate hidden values
		int o = 0;
		for (Neuron n : neurons) {
			for (int i = 0; i < n.connections.length; i++) {
				if (n.connections[i] < 0) {
					// Connection to input node
					hiddenVals[o] += input[n.connections[i] + MyEA.inputLayerSize] * n.weights[i];
				}
			}
			o++;
		}
		
		double c0 = 0;
		for (Neuron n : neurons) {
			for (int i = 0; i < n.connections.length; i++) {
				if (n.connections[i] == 0) {
					// Connection to acceleration output node
					c0 += input[n.connections[i]] * n.weights[i];
				}
			}
		}
//		if (acc > 0)
//			acc = 1;
//		else
//			acc = -1;
//		return acc;
		// Experimental
		c0 = Math.abs(c0);
		if(c.distance > 50){
			// Accelerate if needed
			if(sensors.getSpeed() < 200 * c0){
				return 1;
			} else {
				return 0;
			}
		} else {
			if(sensors.getSpeed() > 100 * c0){
				// Brake
				return -1;
			} else {
				// Continue
				return 1; 
			}
		}
	}

	@Override
	public double getSteering(SensorModel sensors) {
		Corner c = trackmap.getNextCorner(sensors.getDistanceFromStartLine());
		double[] input = new double[MyEA.inputLayerSize];
		input[0] = sensors.getSpeed();
		input[1] = c.distance;
		input[2] = c.sharpness;
		input[3] = sensors.getTrackPosition();
		input[4] = sensors.getAngleToTrackAxis();
		Neuron[] neurons = genome.hiddenLayer;
		double[] hiddenVals = new double[neurons.length];
		// Calculate hidden values
		int o = 0;
		for (Neuron n : neurons) {
			for (int i = 0; i < n.connections.length; i++) {
				if (n.connections[i] < 0) {
					// Connection to input node
					hiddenVals[o] += input[n.connections[i] + MyEA.inputLayerSize] * n.weights[i];
				}
			}
			o++;
		}
		
		double c1 = 0;
		for (Neuron n : neurons) {
			for (int i = 0; i < n.connections.length; i++) {
				if (n.connections[i] == 1) {
					// Connection to acceleration output node
					c1 += input[n.connections[i]] * n.weights[i];
				}
			}
		}
		// return c1;
		// Experimental
		c1 = Math.abs(c1);
		if(c.distance > 0){
			// You are approaching a corner
			if(c.sharpness < 0){
				//Corner to the left
				if( sensors.getTrackPosition() < c1 ){
					// Move to the right side of the track
					return utils.moveTowardsTrackPosition( sensors, Math.abs(sensors.getTrackPosition() - c1), c1 );	
				} else {
					// Straight on
					return utils.alignToTrackAxis(sensors, Math.abs(sensors.getAngleToTrackAxis()));
				}
			} else {
				//Corner to the right
				if( sensors.getTrackPosition() > -1 * c1 ){
					// Move to the left side of the track
					return utils.moveTowardsTrackPosition( sensors, Math.abs(sensors.getTrackPosition() - c1), -1 * c1 );	
				} else {
					// Straight on
					return utils.alignToTrackAxis(sensors, Math.abs(sensors.getAngleToTrackAxis()));
				}
			}
		} else {
			// You are in a corner, so move to the track axis
			return utils.moveTowardsTrackPosition(sensors, 2*Math.abs(sensors.getTrackPosition()), 0);
		}
		
	}

	@Override
	public void loadGenome(IGenome genome) {
		if (genome instanceof DriverGenome) {
			this.genome = (DriverGenome) genome;
		} else {
			System.err.println("Invalid Genome assigned");
			System.err.println(genome.getClass());
		}
	}

	@Override
	public String getDriverName() {
		return this.name;
	}

	
	
	/*
	 * The following methods are only here as a reminder that you can,
	 * and may change all driver methods, including the already implemented
	 * ones, such as those beneath.
	 */
	public void controlQualification(Action action, SensorModel sensors) {	
		super.controlQualification(action, sensors);
	}
	
	public void controlRace(Action action, SensorModel sensors) {
		super.controlRace(action, sensors);
	}
	
	public void defaultControl(Action action, SensorModel sensors){
		super.defaultControl(action, sensors);
	}
}
